#include "MyMath.h"

Vector4 Vector4::operator* (const Matrix4X4& M) const
{
	Vector4 Result;
	Result.m_x = this->m_x * M.m_00 + this->m_y * M.m_10 + this->m_z * M.m_20 + this->m_w * M.m_30;
	Result.m_y = this->m_x * M.m_01 + this->m_y * M.m_11 + this->m_z * M.m_21 + this->m_w * M.m_31;
	Result.m_z = this->m_x * M.m_02 + this->m_y * M.m_12 + this->m_z * M.m_22 + this->m_w * M.m_32;
	Result.m_w = this->m_x * M.m_03 + this->m_y * M.m_13 + this->m_z * M.m_23 + this->m_w * M.m_33;
	return Result;
}

void GetRollYawPitch(float& OutPitch, float& OutYaw, float& OutRoll, const Matrix3X3& InMatrix)
{
	assert(0);
}

Matrix3X3 BuildRotationMatrix(float InPitch, float InYaw, float InRoll)
{	
	float CosPitch = cos(InPitch);
	float SinPitch = sin(InPitch);
	Matrix3X3 RotationX(1.0f, 0.0f, 0.0f,
						0.0f, CosPitch, SinPitch,
						0.0f, -SinPitch,CosPitch);
	
	float CosYaw = cos(InYaw);
	float SinYaw = sin(InYaw);
	Matrix3X3 RotationY(CosYaw, 0.0f, -SinYaw,
						0.0f, 1.0f, 0.0f,
						SinYaw, 0.0f, CosYaw);

	float CosRoll = cos(InRoll);
	float SinRoll = sin(InRoll);
	Matrix3X3 RotationZ(CosRoll, SinRoll, 0.0f,
						-SinRoll, CosRoll, 0.0f,
						0.0f, 0.0f, 1.0f);

	return RotationZ*RotationX*RotationY;
}
